#include "px4_interface.h"
#include "zmq_client.h"
#include <thread>

using std::cout; 
using std::endl;

enum TestPhase {
    HOLD = 0, 
    SEPARATE,
    ALIGN,
    POINT
};

TestPhase test_phase_{HOLD};

float set_alt_by_id(uint8_t id, float alt) {
    float ret{alt};
    id = id < 10 ? id : id - 9;
    if (id <= 2) {
        ret = alt + 2;
    }
    else if (id >= 8) {
        ret = alt - 2;
    }
    return ret;
}

void vehicle_node(uint8_t id, float spd, float alt) {

    alt = set_alt_by_id(id, alt);

    Px4Interface px4_interface(26539+id, 24579+id, "127.0.0.1");
    UavStatus px4_info;

    mavlink_set_position_target_local_ned_t local_setpoint{};

    local_setpoint.vx = spd;
    local_setpoint.vy = 0;
    local_setpoint.z = -alt;
    local_setpoint.yaw = 0;
    local_setpoint.target_component = MAV_COMP_ID_AUTOPILOT1;
    local_setpoint.type_mask = 0b0000101111100011;
    local_setpoint.coordinate_frame = MAV_FRAME_LOCAL_NED;

    bool group_direct = id > 9;

    uint64_t separate_time{0};
    uint64_t hold_time{0};

    while(true) {
        if (px4_interface.updated(px4_info)) {
            switch (test_phase_) {
                case HOLD: {
                    local_setpoint.vy = 0;
                    local_setpoint.vx = 0;
                    local_setpoint.x = 0;
                    local_setpoint.y = 0;
                    local_setpoint.z = -alt;
                    local_setpoint.type_mask = 0b0000101111100000;
                    separate_time = 0;
                    hold_time = 0;
                    break;
                }
                case SEPARATE: {
                    if (separate_time == 0) {
                        separate_time = currentTimeMs();
                    }
                    if (group_direct) {
                        local_setpoint.vx = -spd;
                    } else {
                        local_setpoint.vx = spd;
                    }
                    local_setpoint.vy = 0;
                    local_setpoint.z = -alt - 1;
                    local_setpoint.type_mask = 0b0000101111100011;
                    if (elapsedTimeMs(separate_time) > 300) {
                        test_phase_ = HOLD;
                        separate_time = 0;
                        std::cout << "change phase to " << (int)test_phase_ << std::endl;
                    }
                    break;
                }
                case ALIGN: {
                    if (hold_time == 0) {
                        hold_time = currentTimeMs();
                    }
                    if (group_direct) {
                        local_setpoint.vx = 0.5;
                    } else {
                        local_setpoint.vx = -0.5;
                    }
                    local_setpoint.vy = 0;
                    local_setpoint.z = -alt;
                    local_setpoint.type_mask = 0b0000101111100011;
                    if (elapsedTimeMs(hold_time) > 30000) {
                        test_phase_ = HOLD;
                        hold_time = 0;
                        std::cout << "change phase to " << (int)test_phase_ << std::endl;
                    }
                    break;
                }
                case POINT: {
                    local_setpoint.x = 10;
                    local_setpoint.y = 0;
                    local_setpoint.z = -alt;
                    local_setpoint.type_mask = 0b0000101111111000;
                    separate_time = 0;
                    hold_time = 0;
                    break;
                }
                default: {
                    local_setpoint.x = 0;
                    local_setpoint.y = 0;
                    local_setpoint.z = -alt;
                    local_setpoint.type_mask = 0b0000101111100011;
                }
            }
            px4_interface.send_local_target(local_setpoint);

        }

        usleep(50 * 1000);
    }
}

int main() {

    uint8_t swarm_num = 20;
    float spd = 10;
    float alt = 5;

    for (uint8_t id = 1; id <= swarm_num; id++) {
        std::cout << "test start for id " << (int)id << std::endl;

        std::thread t(vehicle_node, id, spd, alt);
        t.detach();
    }

    char key_char{'0'};

    while (true) {
        std::cout << "input phase: " << std::endl;
        std::cin >> key_char;
        test_phase_ = (TestPhase)(atoi(&key_char));
        std::cout << "change phase to " << key_char << std::endl;
    }

}